close all; clear; clc;
%% theoretical model

d1 = 0.089159; d4 = 0.10915; d5 = 0.09465; d6 = 0.0823;
a3 = 0.425; a4 = 0.39225;

Alpha = [0, -pi/2, 0, 0, pi/2, -pi/2];
A = [0, 0, a3, a4, 0, 0];
D = [d1, 0, 0, d4, d5, d6];
Theta = [0, -pi/2, 0, pi/2, 0, 0];

for i = 1:6
    L(i) = Link('alpha',Alpha(i),'a', A(i),'d', D(i)); L(i).offset = Theta(i); L(i).qlim=[-pi, pi]; L(i).mdh = 1;
end

robot = SerialLink(L, 'name', 'UR');
robot.tool.t = [0, -0.03, 0.35];
q0 = zeros(1, 6);

p_object = [0.5, 0, 0.3];
R1 = rotx(pi) * rotx(pi/3) * roty(pi/4);
R2 = rotx(pi) * rotx(-pi/6) * roty(pi/3);
R3 = rotx(pi) * roty(pi/3) * rotz(pi/6);
R4 = rotx(pi);
TR1 = rt2tr(R1, p_object);
TR2 = rt2tr(R2, p_object);
TR3 = rt2tr(R3, p_object);
TR4 = rt2tr(R4, p_object);
TR5 = rt2tr(R4, p_object + [0.05, 0, 0]);
TR6 = rt2tr(R4, p_object + [0, 0.05, 0]);
TR7 = rt2tr(R4, p_object + [0, 0, 0.05]);


q1 = robot.ikine(TR1);
q2 = robot.ikine(TR2);
q3 = robot.ikine(TR3);
q4 = robot.ikine(TR4);
q5 = robot.ikine(TR5);
q6 = robot.ikine(TR6);
q7 = robot.ikine(TR7);

Q = [q0; q1; q0; q2; q0; q3; q0; q4; q4; q5; q4; q6; q4; q7; q4; q0];

%% C
Rc = rotz(-pi/2);
tc = [0.5; 0.2; 0];
Tc = rt2tr(Rc, tc);
p{1} = [0; 0; 0];
p{2} = [0.04; 0; 0];
p{3} = p{1};
p{4} = [0; -0.05; 0];
p{5} = [0.04; -0.05; 0];

%% I
ti = [0.05; 0; 0];
p{6} = ti + [0; 0; 0];
p{7} = ti + [0.02; 0; 0];
p{8} = ti + [0.01; 0; 0];
p{9} = ti + [0.01; -0.05; 0];
p{10} = ti + [0; -0.05; 0];
p{11} = ti + [0.02; -0.05; 0];

%% T
tt = ti + [0.03; 0; 0];
p{12} = tt + [0; 0; 0];
p{13} = tt + [0.04; 0; 0];
p{14} = tt + [0.02; 0; 0];
p{15} = tt + [0.02; 0.02; 0];
p{16} = tt +  [0.02; -0.05; 0];
p{17} = tt +  [0.04; -0.05; 0];

%% R
tr = tt + [0.05; 0; 0];
p{18} = tr + [0; 0; 0];
p{19} = tr + [0; -0.05; 0];
p{20} = tr + [0; 0; 0];
p{21} = tr + [0.02; 0; 0];

%% U
tu = tr + [0.03; 0; 0];
p{22} = tu + [0; 0; 0];
p{23} = tu + [0; -0.05; 0];
p{24} = tu + [0.04; -0.05; 0];
p{25} = tu + [0.04; 0; 0];

%% S
ts = tu + [0.05; 0; 0];
p{26} = ts + [0; 0; 0];
p{27} = ts + [0.04; 0; 0];
p{28} = ts + [0; 0; 0];
p{29} = ts + [0; -0.025; 0];
p{30} = ts + [0.04; -0.025; 0];
p{31} = ts + [0.04; -0.05; 0];
p{32} = ts + [0; -0.05; 0];
p{33} = ts + [0.10; -0.05; 0];

for i = 1:length(p)
    T{i} = Tc * transl(p{i}) * trotx(pi) * trotz(-pi/2);
end

N = 10;
T_all = [];
for i = 1:length(p) - 1
    TC = ctraj(T{i}, T{i + 1}, N);
    T_all = cat(3, T_all, TC);
end

t = 0:0.08:30;
q_all = zeros(length(t), 6);

qe = robot.ikine(T_all);

q_all(1:length(T_all), :) = qe;
q_all(length(T_all) + 1:end, :) = q_all(length(T_all) + 1:end, :) + qe(end, :);
q_all_timeseries = timeseries(q_all, t);